#include "include/KalmanFilters.h"
#include "include/VectorTest.h"

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;
using namespace Ei;
using namespace Ogre;

const unsigned NX = 2;	
//
// Predict Model
//
BasicPredict::BasicPredict() : Linear_predict_model(NX,1)
{
	
}

//
// observeModel
//
BasicObserve::BasicObserve () : Linear_uncorrelated_observe_model(NX,1)
{			

}


//
// Full Filter
//
NoiseFilter::NoiseFilter() : mFilter(NX) // one state filter
{

	
}

NoiseFilter::~NoiseFilter()
{

}
	
Real NoiseFilter::Predict()
{

	mFilter.predict(mPredict);
	mFilter.update();	
	Vec v(1);	
	v = mFilter.x; 
	return Real(v[0]);
}

void NoiseFilter::Measure(Real data)
{
	// Make an observation
	Vec z(1);
	z[0] = data;
	mFilter.observe(mObserve, z);
	mFilter.update();
}
void NoiseFilter::setDT(Real newdt)
{	
	dt = newdt;
	float Fvv = exp(-dt*V_GAMMA);
	
	mPredict.Fx(0,0) = 1.;//A(x k -1) -> change of the data 0 <-> 1 one is no chh
	mPredict.Fx(0,1) = dt;	
	mPredict.Fx(1,0) = 0;
	mPredict.Fx(1,1) = Fvv;	
}

void NoiseFilter::init()
{	
	//////////////////////////////////////////////////////////
	// setup the observation model
	V_NOISE = 0.8;	// Velocity noise, giving mean squared error bound
	V_GAMMA = 0.8;	// Velocity correlation, giving velocity change time constant
	dt = .1;
	float Fvv = exp(-dt*V_GAMMA);
	
	mPredict.Fx(0,0) = 1.;//A(x k -1) -> change of the data 0 <-> 1 one is no chh
	mPredict.Fx(0,1) = dt;	
	mPredict.Fx(1,0) = 0;
	mPredict.Fx(1,1) = Fvv;
	
	mPredict.q[0] = dt*sqr((1-Fvv)*V_NOISE);
	mPredict.G(0,0) = 0.;
	mPredict.G(1,0) = 1.;
	/////////////////////////////////////////////////////
	//setup the prediction model
	OBS_NOISE = 0.05;	
	// Linear model
	mObserve.Hx(0,0) = 1;
	mObserve.Hx(0,1) = 0.;
	// Observation Noise variance
	mObserve.Zv[0] = sqr(OBS_NOISE);
	
	// Setup the initial state and covariance
	// intialise the direct and indirect kalman filter
	Vec x_true (NX);
	// Filter's Initial state uncertainty: System state is unknown
	const Float i_P_NOISE = 0.1;	
	const Float i_V_NOISE = 0.1;

	// True State to be observed
	x_true[0] = 1.;	// Position
	x_true[1] = 0;
 
	// Construct Prediction and Observation model and filter
	// Give the filter an initial guess of the system state
	Vec x_guess(NX);
	x_guess[0] = -1.;
	x_guess[1] = 0;

	// mDirectFilter filter construct and initialize with initial state guess
	mFilter.X.clear();
	mFilter.X(0,0) = sqr(i_P_NOISE);
	mFilter.X(1,1) = sqr(i_V_NOISE);
		
	mFilter.init_kalman (x_guess, mFilter.X);

}

void NoiseFilter::reinit()
{
	//////////////////////////////////////////////////////////
	// setup the observation model
	//V_NOISE = 0.8;	// Velocity noise, giving mean squared error bound
	//V_GAMMA = 0.8;	// Velocity correlation, giving velocity change time constant
	//dt = .1;
	float Fvv = exp(-dt*V_GAMMA);
	
	mPredict.Fx(0,0) = 1.;//A(x k -1) -> change of the data 0 <-> 1 one is no chh
	mPredict.Fx(0,1) = dt;	
	mPredict.Fx(1,0) = 0;
	mPredict.Fx(1,1) = Fvv;
	
	mPredict.q[0] = dt*sqr((1-Fvv)*V_NOISE);
	mPredict.G(0,0) = 0.;
	mPredict.G(1,0) = 1.;
	/////////////////////////////////////////////////////
	//setup the prediction model
	//OBS_NOISE = 0.05;	
	// Linear model
	mObserve.Hx(0,0) = 1;
	mObserve.Hx(0,1) = 0.;
	// Observation Noise variance
	mObserve.Zv[0] = sqr(OBS_NOISE);

}
